#include "actuator/kinematic_normal.h"

using namespace actuator;

AMRKinematicModeNormal::AMRKinematicModeNormal()
{
    max_steer_speed_ = 1.0f;
}

AMRKinematicModeNormal::~AMRKinematicModeNormal()
{
}

bool AMRKinematicModeNormal::Init(std::string &path)
{
    max_steer_angle_ = 100.0f * M_PI / 180.0f;
    return true;
}

void AMRKinematicModeNormal::CalcSteerCmd(const float speed, const float angle_speed, const float speed_towards, float &x, float &y, SteerCmd &steer_cmd, eMoveMode mode)
{
    double vx = 0.0f, vy = 0.0f;
    steer_cmd.steer_speed = 0.0f;
    switch (mode)
    {
    case eMoveMode::TOWARD:
        steer_cmd.steer_angle = speed_towards;
        steer_cmd.steer_speed = speed;
        break;

    case eMoveMode::TURN:
        if (fabs(speed_towards) > FLT_EPSILON)
        {
            double v_forward = cos(speed_towards);
            double v_rot = sin(speed_towards);
            double radius = v_forward / v_rot;

            if (fabs(radius) < min_radius_)
                radius = (speed_towards > 0.0) ? (min_radius_) : (-min_radius_);
            double w = speed / radius;
            steer_cmd.steer_angle = atan2(x, radius - y);
            steer_cmd.steer_speed = w * fabs((radius - y) / cos(steer_cmd.steer_angle));
            restain_steer_angle(steer_cmd.steer_angle, steer_cmd.steer_speed);
        }
        else
        {
            steer_cmd.steer_speed = speed;
            steer_cmd.steer_angle = 0.0f;
        }
        break;
    case eMoveMode::ROTATE:
        steer_cmd.steer_angle = atan2(x, -y);
        vx = angle_speed * y;
        vy = angle_speed * x;
        steer_cmd.steer_speed = (angle_speed > 0 ? 1 : -1) * sqrt(pow(vx, 2) + pow(vy, 2));
        restain_steer_angle(steer_cmd.steer_angle, steer_cmd.steer_speed);
        break;
    case eMoveMode::HEADFOLLOW:
        vx = speed * cos(speed_towards) - angle_speed * y;
        vy = speed * sin(speed_towards) + angle_speed * x;

        steer_cmd.steer_speed = sqrt(pow(vx, 2) + pow(vy, 2));
        steer_cmd.steer_angle = atan2(vy, vx);

        if (fabs(steer_cmd.steer_angle) > max_steer_angle_ && fabs(steer_cmd.steer_angle) < 120 * M_PI / 180.0f)
        {
            steer_cmd.steer_angle = (steer_cmd.steer_angle > 0.0f ? 1.0 : -1.0) * max_steer_angle_;
        }
        else
        {
            restain_steer_angle(steer_cmd.steer_angle, steer_cmd.steer_speed);
        }

        if (speed * steer_cmd.steer_speed < 0.0f && fabs(steer_cmd.steer_angle) > M_PI_4)
        {
            steer_cmd.steer_speed = -steer_cmd.steer_speed;
            if (steer_cmd.steer_angle < 0)
            {
                steer_cmd.steer_angle = steer_cmd.steer_angle + M_PI;
            }
            else
            {
                steer_cmd.steer_angle = steer_cmd.steer_angle - M_PI;
            }
        }

        if (speed == 0.0)
        {
            steer_cmd.steer_angle = speed_towards;
        }
        break;

    default:
        steer_cmd.steer_speed = 0.0f;
        steer_cmd.steer_angle = 0.0f;
        break;
    }

    if (speed_towards * steer_cmd.steer_angle < -FLT_EPSILON && fabs((fabs(steer_cmd.steer_angle) - M_PI_2)) < 0.000001)
    {
        steer_cmd.steer_angle = speed_towards;
        steer_cmd.steer_speed = -steer_cmd.steer_speed;
    }
}

void AMRKinematicModeNormal::restain_steer_angle(float &angle, float &speed)
{
    if (angle - M_PI_2 > FLT_EPSILON)
    {
        angle = angle - M_PI;
        speed = -speed;
    }
    else if (angle + M_PI_2 < -FLT_EPSILON)
    {
        angle = angle + M_PI;
        speed = -speed;
    }
}

void AMRKinematicModeNormal::KinematicMode(float speed, float angle_speed, float speed_towards, eMoveMode mode, AMRCmdAndState &amr_state)
{
    if (amr_state.steer_param.size() != amr_state.steer_state.size())
        return;

    size_t size = amr_state.steer_state.size();

    bool speed_out = false;
    for (int i = 0; i < size; i++)
    {
        if (fabs(amr_state.steer_state[i].steer_speed) > max_steer_speed_ * 1.1f)
        {
            speed_out = true;
            break;
        }
    }

    if (mode == eMoveMode::STILL || speed_out)
    {
        amr_state.steer_cmd.clear();
        for (int i = 0; i < size; i++)
        {
            SteerCmd cmd;
            cmd.steer_speed = 0.0f;
            cmd.steer_angle = amr_state.steer_state[i].steer_angle;
            amr_state.steer_cmd.push_back(cmd);
        }
        return;
    }

    std::vector<SteerCmd> calc_cmds(size);
    for (int i = 0; i < size; i++)
    {
        CalcSteerCmd(speed, angle_speed, speed_towards, amr_state.steer_param[i].pos_x, amr_state.steer_param[i].pos_y, calc_cmds[i], mode);
    }

    if (last_mode_ != mode)
    {
        control_stage_ = 0;
        last_mode_ = mode;
    }

    if (control_stage_ == 0)
    {
        bool keep_still = true;
        amr_state.steer_cmd.clear();
        for (int i = 0; i < size; i++)
        {
            SteerCmd steer_cmd_;
            steer_cmd_.steer_speed = 0;
            steer_cmd_.steer_angle = amr_state.steer_state[i].steer_angle;
            amr_state.steer_cmd.push_back(steer_cmd_);
        }
        for (int i = 0; i < size; i++)
        {
            if (fabs(amr_state.steer_state[i].steer_speed) > zero_speed_)
            {
                keep_still = false;
                break;
            }
        }
        if (keep_still)
        {
            control_stage_ = control_stage_ + 1;
        }
    }

    if (control_stage_ == 1)
    {
        bool keep_still = true;
        amr_state.steer_cmd.clear();
        for (int i = 0; i < size; i++)
        {
            SteerCmd steer_cmd_;
            double delta_angel = calc_cmds[i].steer_angle - amr_state.steer_state[i].steer_angle;
            delta_angel = com_.AngleConstraintToPI(delta_angel);
            if ((fabs(delta_angel) > (5 * M_PI / 180.0f)) && keep_still)
            {
                keep_still = false;
            }
            steer_cmd_.steer_speed = 0;
            steer_cmd_.steer_angle = calc_cmds[i].steer_angle;
            amr_state.steer_cmd.push_back(steer_cmd_);
        }

        for (int i = 0; i < size; i++)
        {
            if (fabs(amr_state.steer_state[i].steer_speed) > 0.01 && keep_still)
            {
                keep_still = false;
                break;
            }
        }

        if (keep_still)
        {
            control_stage_ = control_stage_ + 1;
        }
    }
    float angle_out_data = M_PI_2;
    if (control_stage_ == 2)
    {
        bool is_angle_out = false;
        float k_angle = 1;
        float max_angle_error = 0;
        float scale_speed = speed;

        for (int i = 0; i < size; i++)
        {
            double delta_angel = calc_cmds[i].steer_angle - amr_state.steer_state[i].steer_angle;
            delta_angel = com_.AngleConstraintToPI(delta_angel);

            max_angle_error = std::max(max_angle_error, (float)fabs(delta_angel));
        }
        if (max_angle_error > angle_out_data)
        {
            is_angle_out = true;
        }
        else
        {
            k_angle = 1 - max_angle_error / angle_out_data;
        }

        k_angle = com_.ValueConstraintMinToMax(k_angle, 0, 1);

        amr_state.steer_cmd.clear();
        for (int i = 0; i < size; i++)
        {
            SteerCmd steer_cmd_;
            scale_speed = k_angle * calc_cmds[i].steer_speed;
            double delta_angel = calc_cmds[i].steer_angle - amr_state.steer_state[i].steer_angle;
            delta_angel = com_.AngleConstraintToPI(delta_angel);

            steer_cmd_.steer_speed = scale_speed;
            steer_cmd_.steer_angle = calc_cmds[i].steer_angle;
            amr_state.steer_cmd.push_back(steer_cmd_);
        }

        if (is_angle_out || (fabs(speed)<0.01 && fabs(angle_speed)<0.001))
        {
            control_stage_ = 0;
            amr_state.steer_cmd.clear();
            for (int i = 0; i < size; i++)
            {
                SteerCmd steer_cmd_;
                steer_cmd_.steer_speed = 0;
                steer_cmd_.steer_angle = calc_cmds[i].steer_angle;;
                
                amr_state.steer_cmd.push_back(steer_cmd_);
            }
        }
    }
}

void AMRKinematicModeNormal::VehicleInverseMode(AMRCmdAndState &amr_state, float &speed, float &angle_speed, float &steer_angle)
{
    if (amr_state.steer_state.empty() || amr_state.steer_state.size() != amr_state.steer_param.size())
    {
        speed = 0;
        angle_speed = 0;
        steer_angle = 0;
        return;
    }

    float vx = 0.0f, vy = 0.0f, w = 0.0f, steer = 0.0f;
    for (int i=0;i<amr_state.steer_param.size();i++)
    {
        float v_x, v_y, ww;
        
        v_x = amr_state.steer_state[i].steer_speed * cos(amr_state.steer_state[i].steer_angle);
        v_y = amr_state.steer_state[i].steer_speed * sin(amr_state.steer_state[i].steer_angle);
        ww  = amr_state.steer_state[i].steer_speed * sin(com_.getVectorAngle2d(amr_state.steer_state[i].steer_angle, amr_state.steer_param[i].pos_x, amr_state.steer_param[i].pos_y)) 
              / (sqrt(pow(amr_state.steer_param[i].pos_x, 2)+pow(amr_state.steer_param[i].pos_y, 2)));

        vx += v_x;
        vy += v_y;
        w  += ww;
        steer += amr_state.steer_state[i].steer_angle;
    }

    vx = vx/amr_state.steer_param.size();
    vy = vy/amr_state.steer_param.size();
    w  =  w/amr_state.steer_param.size();
    steer = steer/amr_state.steer_param.size();

    angle_speed = w;
    speed = (sqrt(pow(vx, 2) + pow(vy, 2)));
    float angle = com_.AngleConstraintToPI(atan2(vy, vx) - steer);
    
    if (fabs(angle) > M_PI_2)
    {
        speed = -speed;
        steer_angle = com_.AngleConstraintToPI(atan2(vy, vx) + M_PI);
    }
    else
    {
        steer_angle = atan2(vy, vx);
    }

    if (fabs(speed) < 0.001f)
    {
        steer_angle = steer;
    }
}